{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [],
   "source": [
    "\"\"\"\n",
    "Stanley method\n",
    "\"\"\"\n",
    "\n",
    "from scipy.spatial import KDTree\n",
    "import numpy as np\n",
    "import math\n",
    "import copy\n",
    "import matplotlib\n",
    "import matplotlib.pyplot as plt\n",
    "%matplotlib qt5\n",
    "\n",
    "\n",
    "\n",
    "plt.ion()\n",
    "# plt.figure(figsize=(18, 3))\n",
    "max_steer = np.radians(25.0)  # [rad] max steering angle\n",
    "\n",
    "\n",
    "def normalize_angle(angle):\n",
    "    \"\"\"\n",
    "    Normalize an angle to [-pi, pi].\n",
    "    :param angle: (float)\n",
    "    :return: (float) Angle in radian in [-pi, pi]\n",
    "    \"\"\"\n",
    "    while angle > np.pi:\n",
    "        angle -= 2.0 * np.pi\n",
    "\n",
    "    while angle < -np.pi:\n",
    "        angle += 2.0 * np.pi\n",
    "\n",
    "    return angle\n",
    "\n",
    "\n",
    "class UGV_model:\n",
    "    def __init__(self, x0, y0, theta0, L, v0, T):  # L:wheel base\n",
    "        self.x = x0  # X\n",
    "        self.y = y0  # Y\n",
    "        self.theta = theta0  # headding\n",
    "        self.l = L  # wheel base\n",
    "        self.v = v0  # speed\n",
    "        self.dt = T  # decision time periodic\n",
    "\n",
    "    def update(self, vt, deltat):  # update ugv's state\n",
    "        dx = self.v*np.cos(self.theta)\n",
    "        dy = self.v*np.sin(self.theta)\n",
    "        dtheta = self.v*np.tan(deltat)/self.l\n",
    "        self.x += dx*self.dt\n",
    "        self.y += dy*self.dt\n",
    "        self.theta += dtheta*self.dt\n",
    "\n",
    "    def plot_duration(self):\n",
    "        plt.scatter(self.x, self.y, color='r')\n",
    "        # plt.axis([self.x-9, self.x+9, -3, 3])\n",
    "#         plt.axis([self.x-9, self.x+9, -10, 10])\n",
    "\n",
    "\n",
    "\n",
    "# set reference trajectory\n",
    "refer_path = np.zeros((1000, 2))\n",
    "refer_path[:, 0] = np.linspace(0, 100, 1000)\n",
    "refer_head = np.zeros(1000)\n",
    "refer_path[:, 1] = 5*np.sin(refer_path[:, 0]/5.0) + \\\n",
    "    2.5*np.cos(refer_path[:, 0]/2.0)  # 生成正弦轨迹\n",
    "\n",
    "refer_tree = KDTree(refer_path)\n",
    "\n",
    "plt.plot(refer_path[:, 0], refer_path[:, 1], '-.b', linewidth=5.0)\n",
    "ugv = UGV_model(0, 1.0, 0, 2.0, 2.0, 0.1)\n",
    "k = 2.0\n",
    "ld = 2.0  # 车前后轮距离，轴距\n",
    "pind = 0\n",
    "ind = 0\n",
    "for i in range(1000):\n",
    "    robot_state = np.zeros(2)\n",
    "    robot_state[0] = ugv.x + ld*np.cos(ugv.theta)\n",
    "    robot_state[1] = ugv.y + ld*np.sin(ugv.theta)\n",
    "    _, ind = refer_tree.query(robot_state)\n",
    "    if ind < pind:\n",
    "        ind = pind\n",
    "    else:\n",
    "        pind = ind\n",
    "\n",
    "    dist = np.linalg.norm(robot_state-refer_path[ind])\n",
    "    dx, dy = refer_path[ind] - robot_state\n",
    "    alpha = math.atan2(dy, dx)\n",
    "    e = np.sign(np.sin(alpha-ugv.theta))*dist\n",
    "    dtheta = normalize_angle(refer_head[ind]-ugv.theta)\n",
    "    theta_d = np.arctan2(k*e, ugv.v)\n",
    "    delta = dtheta+theta_d\n",
    "    ugv.update(2.0, delta)\n",
    "    ugv.plot_duration()\n"
   ]
  }
 ],
 "metadata": {
  "interpreter": {
   "hash": "0c7484b3574347463e16b31029466871583b0d4e5c4ad861e8848f2d3746b4de"
  },
  "kernelspec": {
   "display_name": "Python 3.8.12 ('gobigger')",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.8.12"
  },
  "orig_nbformat": 4
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
